//
// Created by qzj on 2021/4/8.
//

#ifndef SRC_GROUNDPLANEFIT_H
#define SRC_GROUNDPLANEFIT_H

#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include "glog/logging.h"
#include <cmath>
#include <pcl/common/common.h>

// using eigen lib
#include <Eigen/Dense>

class GroundPlaneFit {
    typedef pcl::PointXYZI PointType;
public:
    GroundPlaneFit();
    void setInputCloud(const pcl::PointCloud<PointType>::Ptr& in_cloud);
    void filterGround(pcl::PointCloud<PointType>::Ptr& g_ground_pc,
                      bool &is_stair_flag);
    void filterGround(pcl::PointCloud<PointType>::Ptr& g_ground_pc);
    void setInputCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& in_cloud);
private:
    pcl::PointCloud<PointType> laserCloudIn;
    pcl::PointCloud<PointType> laserCloudIn_org;

    pcl::PointCloud<PointType>::Ptr g_seeds_pc;
    pcl::PointCloud<PointType>::Ptr g_not_ground_pc;

    std::string point_topic_;

    double sensor_height_;
    int num_iter_;
    int num_lpr_;
    double th_seeds_;
    double th_dist_;

    void estimate_plane_(const pcl::PointCloud<PointType>::Ptr &g_ground_pc);
    void extract_initial_seeds_(const pcl::PointCloud<PointType>& p_sorted);

    /*
    @brief Compare function to sort points. Here use z axis.
    @return z-axis accent
*/
    static inline bool point_cmp(PointType a, PointType b){
        return a.z<b.z;
    }

    // Model parameter for ground plane fitting
    // The ground plane model is: ax+by+cz+d=0
    // Here normal:=[a,b,c], d=d
    // th_dist_d_ = threshold_dist - d 
    float d_;
    Eigen::MatrixXf normal_;
    float th_dist_d_;
};


#endif //SRC_GROUNDPLANEFIT_H
